
package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Solenoid;

/**
 *
 * @author mark.oberle
 */
public class Kicker
{
    private static Kicker instance = null;
    private Joystick kickerJoy;
    private int state;
    private boolean diLatchSensor;      // Sensor to Make Sure Kicker is Latched
    private boolean doArmingCylinder;   // Retract Arming Cylinder
    private boolean doTriggerCylinder;  // Extend Trigger Cylinder
    private Solenoid sol01;      // Slot 8 Output 1
    private Solenoid sol02;      // Slot 8 Output 2

    private boolean softKickMode;       // Kick Mode (false = Hard Kick Mode, true = Soft Kick Mode )
    long startTime;                     // Memorized System Milliseconds Elapsed
    long curTime;                       // Current System Milliseconds Elapsed
    long deltaTime;                     // Difference in Milliseconds between Start and Current

    private boolean manualMode;
    private boolean toggleArmingCylinder;
    private boolean toggleTriggerCylinder;
    private boolean toggleArmingCylinderOns;
    private boolean toggleTriggerCylinderOns;

    public Kicker(Joystick shooter)
    {
        kickerJoy = shooter;
        manualMode = false;
        state = 0;
        softKickMode = false;

        sol01 = new Solenoid(1);
        sol02 = new Solenoid(2);
    }


    void Run()
    {

        // ----------------------------------------
        // Update from Physical Inputs




        // ----------------------------------------
        // Kick Mode Setting (by Operator)

        if( kickerJoy.getRawButton(3)){     // Mode = Hard Kick
            softKickMode = false;
            System.out.println("Kicker Mode = Hard Kick");
        }

        if( kickerJoy.getRawButton(2)){     // Mode = Soft Kick
            softKickMode = true;
            System.out.println("Kicker Mode = Soft Kick");
        }


        // ----------------------------------------
        // State Machine

        switch(state){

            // Initial

            case 0:
                if( manualMode ){
                    state = 99;
                    //System.out.println("kicker state = 99 (manual mode");
                }
                else{
                    startTime = System.currentTimeMillis();
                    state = 1;
                    //System.out.println("kicker state = 1");
                }



            // Arm the Kicker
            case 1:
                doTriggerCylinder = false;
                doArmingCylinder = true;
                curTime = System.currentTimeMillis();
                deltaTime = curTime - startTime;        // will always be > 0 (in our lifetime)
                if( diLatchSensor == true || deltaTime > 5000 ){
                    state = 2;
                    //System.out.println("kicker state = 2");
                }
                break;


            // Wait for Operator to Command a Kick
            case 2:
                if( softKickMode )              // Look at the Mode
                    doArmingCylinder = true;
                else
                    doArmingCylinder = false;
                if( !manualMode && kickerJoy.getRawButton(1)){  // Operator Commands a Kick
                    startTime = System.currentTimeMillis();
                    state = 3;
                    //System.out.println("kicker state = 3");
                }

                break;


                // Kicking
            case 3:
                doTriggerCylinder = true;
                doArmingCylinder = false;
                curTime = System.currentTimeMillis();
                deltaTime = curTime - startTime;        // will always be > 0 (in our lifetime)
                if( deltaTime > 1000 ){
                    state = 1;
                    //System.out.println("kicker state = 0");
                }
                break;
                

            // Manual Mode
            case 99:
                // Manual Operation of Trigger Cylinder
                toggleTriggerCylinder = kickerJoy.getRawButton(10);
                if( toggleTriggerCylinder && !toggleTriggerCylinderOns ){
                    if( doTriggerCylinder )
                        doTriggerCylinder = false;
                    else
                        doTriggerCylinder = true;
                    System.out.println("doTriggerCylinder = " + doTriggerCylinder );
                }
                toggleTriggerCylinderOns = toggleTriggerCylinder;

                // Manual Operation of Arming Cylinder
                toggleArmingCylinder = kickerJoy.getRawButton(11);
                if( toggleArmingCylinder && !toggleArmingCylinderOns ){
                    if( doArmingCylinder )
                        doArmingCylinder = false;
                    else
                        doArmingCylinder = true;
                    System.out.println("doArmingCylinder = " + doArmingCylinder );
                }
                toggleArmingCylinderOns = toggleArmingCylinder;

                // Exit Manual Mode
                if( !manualMode ){
                    state = 0;
                    //System.out.println("kicker state = 0");
                }

                break;

            default:
                manualMode = false;
                state = 0;
                break;

        }


        // ----------------------------------------
        // Update the Physical Outputs

        sol01.set(doArmingCylinder);
        sol02.set(doTriggerCylinder);

    }

}
